#include "Physics.h"

#include "Level.h"
#include "CollisionFlags.h"
#include "StatefulUnit.h"
#include "NumberUtility.h"

// the default value, should work well for Mario-style platformers with a tile size of 32-64
#define DEFAULT_GRAVITY 2.0f

#define DEFAULT_GLOBAL_FRICTION 1.0f
#define NORMAL_FRICTION 1.0f
#define ICE_FRICTION 0.25f
#define SLIME_FRICTION 4.0f
#define WATER_FRICTION 0.5f

// movement reduction (pixels) on every check, will be multiplied by surface friction factor
#define MOVEMENT_REDUCTION_GROUND 3.0f
#define MOVEMENT_REDUCTION_AIR 1.0f

// factor by which the speed of a hard collision will be modified
#define BOUNCE_FACTOR_X 0.8f
#define BOUNCE_FACTOR_Y 0.5f
// threshold used to remove small, annoying bounces after a hard-collision
#define BOUNCE_THRESHOLD 2.5f

// use full-shape or rectangular collision detection on units
#define FULL_SHAPE_COLLISION true

using namespace NumberUtility;


Physics* Physics::m_physics = NULL;

Physics::Physics()
{
    m_gravity = DEFAULT_GRAVITY;
    m_friction = DEFAULT_GLOBAL_FRICTION;
}

Physics::~Physics()
{

}

Physics* Physics::GetSingleton()
{
    if (not m_physics)
        m_physics = new Physics();
    return m_physics;
}

//----------------------------
// public functions
//----------------------------


void Physics::calculatePhysics(StatefulUnit* const unit) const
{
    float factorX = NORMAL_FRICTION * m_friction;
    float factorY = 1;

    // Apply physics
    if (not unit->hasCollisionFlag(CollisionFlags::NO_PHYSICS))
    {
        Vector2df tile = unit->m_collisionInfo.getCollisionTile(unit->getPosition(true),diBOTTOM);
        if (tile != Vector2df(-1,-1))
        {

            if (unit->m_collisionInfo.isOnGround(unit->getPosition()))
            {
                switch(unit->getLevel()->tileMap->getPropertyData(tile,PLAYER_REACTION))
                {
                case 2: // water
                    factorX = WATER_FRICTION * m_friction;
                    unit->m_collisionInfo.isInWater = true;
                    break;
                case 3: // ice
                    factorX = ICE_FRICTION * m_friction;
                    break;
                case 4: // slime
                    factorX = SLIME_FRICTION * m_friction;
                    break;
                default:
                    break;
                }
                unit->m_velocity.x -= sign(unit->m_velocity.x) * min(MOVEMENT_REDUCTION_GROUND * factorX, abs(unit->m_velocity.x));
            }
            else
            {
                switch(unit->getLevel()->tileMap->getPropertyData(tile,PLAYER_REACTION))
                {
                case 0: // air
                    // air movement reduction here
                    // I think it might be better to not implement this to preserve an acrade-style movement
                    break;
                case 2: // water
                    factorY = WATER_FRICTION * m_friction;
                    unit->m_collisionInfo.isInWater = true;
                    break;
                }
            }
        }
    }

    // gravity
    if (not unit->hasCollisionFlag(CollisionFlags::NO_GRAVITY))
    {
        unit->m_velocity.y += m_gravity * factorY;
    }


    // Acceleration
    if (unit->getAcceleration().x > 0)
    {
        unit->m_velocity.x += min(max(unit->getMaxAcceleration().x - unit->m_velocity.x,(float)0.0),unit->getAcceleration().x * factorX);
    }
    else if (unit->getAcceleration().x < 0)
    {
        unit->m_velocity.x += max(min(unit->getMaxAcceleration().x - unit->m_velocity.x,(float)0.0),unit->getAcceleration().x * factorX);
    }
    // no friction on y-acceleration (this will be affected by gravity)
    if (unit->getAcceleration().y > 0)
    {
        unit->m_velocity.y += min(max(unit->getMaxAcceleration().y - unit->m_velocity.y,(float)0.0),unit->getAcceleration().y * factorY);
    }
    else if (unit->getAcceleration().y < 0)
    {
        unit->m_velocity.y += max(min(unit->getMaxAcceleration().y - unit->m_velocity.y,(float)0.0),unit->getAcceleration().y * factorY);
    }

    // Reset acceleration
    if (unit->m_velocity.x == unit->getMaxAcceleration().x)
    {
        unit->accelerateX(0,0);
    }
    if (unit->m_velocity.y == unit->getMaxAcceleration().y)
    {
        unit->accelerateY(0,0);
    }

    // Check for tile size
    int maxX = unit->getLevel()->GetTileSize()-1;
    int maxY = unit->getLevel()->GetTileSize()-1;
    if (abs(unit->m_velocity.x) > maxX)
        unit->m_velocity.x = sign(unit->m_velocity.x) * maxX;
    if (abs(unit->m_velocity.y) > maxY)
        unit->m_velocity.y = sign(unit->m_velocity.y) * maxY;
}

void Physics::checkHeroCollision(StatefulUnit* const unit, StatefulUnit* const hero) const
{
    SimpleDirection dir = unit->getCollision()->directionTest(hero->getCollision(),unit->getPosition(),hero->getPosition(),FULL_SHAPE_COLLISION);
    if (dir != diNONE)
        unit->hit(dir,hero,"Hero");
}

void Physics::checkUnitCollision(StatefulUnit* const unit, vector< BaseUnit* >* const enemies) const
{
    vector< BaseUnit* >::iterator I;
    for (I = enemies->begin(); I < enemies->end(); ++I)
    {
        StatefulUnit* current = (StatefulUnit*)(*I);


        SimpleDirection dir = unit->getCollision()->directionTest(current->getCollision(),unit->getPosition(),current->getPosition(),FULL_SHAPE_COLLISION);
        if (dir != diNONE)
            unit->hit(dir,current);
    }
}

void Physics::checkTriggerCollision(BaseUnit* const unit, StatefulUnit* hero) const
{
    Vector2df heroOffset = hero->getCollision()->getRegionOffset();
    // simple rectangular collision, direction does not matter
    if (((hero->getPosition().x + heroOffset.x - unit->getPosition().x) < unit->getWidth() && (unit->getPosition().x - (hero->getPosition().x + heroOffset.x)) < hero->getWidth()) &&
        ((hero->getPosition().y + heroOffset.y - unit->getPosition().y) < unit->getHeight() && (unit->getPosition().y - (hero->getPosition().y + heroOffset.y)) < hero->getHeight()))
    {
        unit->hit(diNONE,hero);
    }
}

void Physics::checkMapCollision(StatefulUnit* const unit) const
{
    unit->m_collisionInfo.clear();
    if (not unit->hasCollisionFlag(CollisionFlags::NO_CHECK_X) && unit->getSpeed().x != 0)
        checkMapCollisionX(unit,unit->getLevel()->tileMap,unit->m_collisionInfo,true);
    if (not unit->hasCollisionFlag(CollisionFlags::NO_CHECK_Y) && unit->getSpeed().y != 0)
        checkMapCollisionY(unit,unit->getLevel()->tileMap,unit->m_collisionInfo,true);
    unit->m_collisionInfo.calculateTotalCorrection();

    unit->hitMap(unit->getLevel()->tileMap,&unit->m_collisionInfo);
}

//----------------------------
// utility functions
//----------------------------

Vector2df Physics::hardCollision(Vector2df& speed, const Vector2df& correction, CRfloat bounceXoverride, CRfloat bounceYoverride)
{
    Vector2df result = speed;

    if (correction.x != 0) // x-collision
    {
        float bounceFactorX = BOUNCE_FACTOR_X;
        if (bounceXoverride != 0.0f)
            bounceFactorX = bounceXoverride;

        speed.x *= -1 * bounceFactorX;
        if (abs(speed.x) < BOUNCE_THRESHOLD)
        {
            speed.x = 0;
            result.x += correction.x;
        }
        else
        {
            result.x = result.x + 2.0f * correction.x; // this is clarified best by using a sketch
        }
    }
    if (correction.y != 0) // y-collision
    {
        float bounceFactorY = BOUNCE_FACTOR_Y;
        if (bounceYoverride != 0.0f)
            bounceFactorY = bounceYoverride;

        speed.y *= -1 * bounceFactorY;
        if (abs(speed.y) < BOUNCE_THRESHOLD)
        {
            speed.y = 0;
            result.y += correction.y;
        }
        else
        {
            result.y = result.y + 2.0f * correction.y;
        }
    }
    return result;
}

Vector2df Physics::softCollision(Vector2df& speed, const Vector2df& correction)
{
    if (correction.x != 0)
        speed.x = 0;
    if (correction.y != 0)
        speed.y = 0;
    return (speed + correction);
}

//----------------------------
// private functions
//----------------------------

void Physics::checkMapCollisionX(const StatefulUnit* const unit, const MapData* const data, CollisionObject &obj, bool returnNoColTiles) const
{
    int column, topRow, bottomRow = 0;
    Vector2df correction = Vector2df(0,0);

    // The projected position of the unit
    Vector2df proPos = unit->getPosition() + unit->getPositionOffset();
    proPos.x += unit->getSpeed().x;

    if (unit->getSpeed().x > 0)
        proPos.x += unit->getWidth() - 1;

    // column of tiles to check
    column = floor(proPos.x / (float)data->getTileSize());

    // row range to check
    topRow = floor(proPos.y / (float)data->getTileSize());
    bottomRow = floor((proPos.y + unit->getHeight() - 2) / (float)data->getTileSize());

    // go through tiles and call collision routines accordingly
    // store correction in collisionObject obj
    for (int I = topRow; I <= bottomRow; ++I)
    {
        correction = Vector2df(0,0);

        switch (data->getCollisionData(column,I))
        {
        case 1: // rectangle
            correction.x = rectangular(proPos.x,column,unit->getSpeed().x,data->getTileSize());
            if (not doubleCheck(data,Vector2df(column,I),correction))
                continue;
            break;
        case 2: // bottom-right
            continue;
            break;
        case 3: // bottom-left
            continue;
            break;
        default: // no collision
            correction = Vector2df(0,0);
        }

        // add correction to CollisionObject
        if (returnNoColTiles || correction.x != 0)
            obj.addTileX(Vector2df(column,I),correction);
    }
}

void Physics::checkMapCollisionY(const StatefulUnit* const unit, const MapData* const data, CollisionObject &obj, bool returnNoColTiles) const
{
    int row, columnLeft, columnRight = 0;
    Vector2df correction = Vector2df(0,0);
    int slant = 0;

    // The projected position of the unit
    Vector2df proPos = unit->getPosition() + unit->getPositionOffset();
    proPos.y += unit->getSpeed().y;

    if (unit->getSpeed().y >= 0)
        proPos.y += unit->getHeight() - 1;

    // Range of colliding tiles
    row = floor((proPos.y) / (float)data->getTileSize());
    columnLeft = floor(proPos.x / (float)data->getTileSize());
    columnRight = floor((proPos.x + unit->getWidth() - 2) / (float)data->getTileSize()); // -2 instead -1 to prevent clipping errors

    for (int I = columnLeft; I <= columnRight; ++I)
    {
        switch (data->getCollisionData(I,row))
        {
        case 1: // rectangle
            correction.y = rectangular(proPos.y,row,unit->getSpeed().y,data->getTileSize());
            break;
        case 6: // one-way
            correction.y = oneWay(proPos.y,row,unit->getSpeed().y,data->getTileSize());
            break;
        default: // no collision
            correction.y = 0;
        }

        if (returnNoColTiles || correction.y != 0)
            obj.addTileY(Vector2df(I,row),correction);
    }

    /// Slants will be checked with x-speed applied
    proPos.x += unit->getSpeed().x;

    Vector2di slantTile(-1,-1);

    // Range of colliding tiles
    columnLeft = floor(proPos.x / (float)data->getTileSize());
    columnRight = floor((proPos.x + (float)unit->getWidth() - 2) / (float)data->getTileSize());

    // double-check slants to avoid passing-through
    for (int I = columnLeft; I <= columnRight; ++I)
    {
        correction = Vector2df(0,0);

        switch (data->getCollisionData(I,(row-1)))
        {
        case 2: // bottom-right
            correction.y = slantBottomRight(proPos.x + unit->getWidth() - 1, proPos.y, I, (row-1), data->getTileSize(),unit->getSpeed().y);
            slant = NumberUtility::sign(unit->getSpeed().x);
            break;
        case 3: // bottom-left
            correction.y = slantBottomLeft(proPos.x, proPos.y, I, (row-1), data->getTileSize(),unit->getSpeed().y);
            slant = NumberUtility::sign(unit->getSpeed().x) * -1;
            break;
        default:
            correction.y = 0;
        }

        if (correction.y != 0)
        {
            //obj.clear();
            obj.addTileY(Vector2df(I,(row-1)),correction);
            obj.slantCollision = slant;
            slantTile = Vector2di(I,(row-1));
            //return;
        }

        correction = Vector2df(0,0);

        switch (data->getCollisionData(I,row))
        {
        case 2: // bottom-right
            correction.y = slantBottomRight(proPos.x + unit->getWidth() - 1, proPos.y, I, row, data->getTileSize(),unit->getSpeed().y);
            slant = NumberUtility::sign(unit->getSpeed().x);
            break;
        case 3: // bottom-left
            correction.y = slantBottomLeft(proPos.x, proPos.y, I, row, data->getTileSize(),unit->getSpeed().y);
            slant = NumberUtility::sign(unit->getSpeed().x) * -1;
            break;
        default: // no collision
            continue;
        }

        if (returnNoColTiles || correction.y != 0)
        {
            obj.addTileY(Vector2df(I,row),correction);
            obj.slantCollision = slant;
            slantTile = Vector2di(I,row);
            //return;
        }
    }

    if (slantTile != Vector2di(-1,-1) && unit->getSpeed().x != 0) // we have a slant collision
    {
        // check adjacent tile for end of hill
        Vector2di collisionTile = slantTile + Vector2df(sign(unit->getSpeed().x),0);
        if (obj.hasTileX(collisionTile) != -1 && data->getCollisionData(collisionTile) != 0 && data->getCollisionData(collisionTile.x,collisionTile.y-1) == 0)
        {
            correction.y = rectangular(proPos.y,slantTile.y,unit->getSpeed().y,data->getTileSize());
            obj.addTileY(collisionTile,correction);
        }

    }
}

//----------------------------
// collision detection routines
//----------------------------

float Physics::rectangular(CRfloat pos, CRint tilePos, CRfloat speed, CRint tileSize) const
{
    if (speed >= 0) // moving right
        return (tilePos * tileSize - pos);
    else // moving left
        return ((tilePos + 1) * tileSize - pos);
}

float Physics::oneWay(CRfloat pos, CRint tilePos, CRfloat speed, CRint tileSize) const
{
    // if unit is falling from above treat it as a rectangular tile
    float result = (tilePos * tileSize - pos);
    if (speed >= 0 && speed >= abs(result))
    {
        return result;
    }
    else
        return 0;
}

float Physics::slantBottomRight(CRfloat posX, CRfloat posY, CRint tileX, CRint tileY, CRint tileSize, CRfloat ySpeed) const
{
    float diffX = (posX - tileX * tileSize);
    if (diffX <= tileSize+1) // +1 to extend the valid rate beyond the actual tile to prevent clipping errors on the end of hills
    {
        float result = tileSize - diffX - (posY - tileY * tileSize);
        if ((result > 0) && (ySpeed < 0))
            result = 0;
        return result;
    }
    else
        return 0;
}

float Physics::slantBottomLeft(CRfloat posX, CRfloat posY, CRint tileX, CRint tileY, CRint tileSize, CRfloat ySpeed) const
{
    float diffX = ((tileX + 1) * tileSize - posX);
    if (diffX <= tileSize+1) // +1 to extend the valid rate beyond the actual tile to prevent clipping errors on the end of hills
    {
        float result = tileSize - diffX - (posY - tileY * tileSize);
        if ((result > 0) && (ySpeed < 0))
            result = 0;
        return result;
    }
    else
        return 0;
}

//

bool Physics::doubleCheck(const MapData* const data, const Vector2df& tile, const Vector2df& correction) const
{
    int temp = data->getCollisionData(tile.x + sign(correction.x),tile.y + sign(correction.y));
    if (temp != 0 && temp != 6)
        return false;
    return true;
}






